#include "hardware_interface/ros_interface.h"

RosInterface::RosInterface()
:rclcpp::Node("hardware_interface_node") {
    
   this->InitParameters();
   this->InitPublishers();
   this->InitSubscribers();
   this->InitTimers();
   this->InitHardware();
  
}

RosInterface::~RosInterface() {
    // 析构函数，用于清理资源，例如关闭订阅的话题和发布的消息
}

/**
 * @brief 初始化参数
 *
 * 初始化参数，例如设置默认的串口号和波特率等。
 */
void RosInterface::InitParameters() {
    this->declare_parameter<std::string>("serial_port", "/dev/ttyACM0");
    __port = this->get_parameter("serial_port").as_string();

    this->declare_parameter<int>("baud_rate", 115200);
    __baudrate = this->get_parameter("baud_rate").as_int();

    this->declare_parameter<double>("get_serial_data_poll_hz", 20.0);  // 默认20Hz
    double get_serial_data_poll_hz = get_parameter("get_serial_data_poll_hz").as_double();

    this->declare_parameter<double>("send_velocity_command_poll_hz", 20.0);  // 默认20Hz
    double send_velocity_command_poll_hz = get_parameter("send_velocity_command_poll_hz").as_double();

    this->declare_parameter<double>("odom_pub_hz", 20.0);  // 默认20Hz
    double odom_pub_hz = get_parameter("odom_pub_hz").as_double();

    this->declare_parameter<double>("hardware_state_pub_hz", 10.0);  // 默认10Hz
    double hardware_state_pub_hz = get_parameter("hardware_state_pub_hz").as_double();

    this->declare_parameter<bool>("is_pub_tf", true);
    __is_pub_tf = this->get_parameter("is_pub_tf").as_bool();

    this->declare_parameter<std::string>("frame_id", "odom");
    __frame_id = this->get_parameter("frame_id").as_string();

    this->declare_parameter<std::string>("child_frame_id", "base_link");
    __child_frame_id = this->get_parameter("child_frame_id").as_string();

    this->declare_parameter<double>("track_width", 0.440);
    __track_width = this->get_parameter("track_width").as_double();

    __last_speed_time = this->get_clock()->now();
    __now_speed_time = this->get_clock()->now();

    __last_encoder_counts.left_mm = 0;
    __last_encoder_counts.right_mm = 0;
    __last_encoder_counts.stamp_ros = this->get_clock()->now();

    __get_serial_data_poll_interval_ms = static_cast<int>(1000.0 / get_serial_data_poll_hz); // 50ms 20Hz
    __send_velocity_command_poll_interval_ms = static_cast<int>(1000.0 / send_velocity_command_poll_hz); // 50ms 20Hz
    __odom_pub_poll_interval_ms = static_cast<int>(1000.0 / odom_pub_hz); // 50ms 20Hz
    __hardware_state_pub_poll_interval_ms = static_cast<int>(1000.0 / hardware_state_pub_hz); // 100ms 10Hz

    __tf_broadcaster = std::make_shared<tf2_ros::TransformBroadcaster>(this);
}

/**
 * @brief 初始化发布者
 *
 * 初始化与ROS通信相关的发布者，包括里程计、位置信息和硬件状态等发布者。
 */
void RosInterface::InitPublishers() {
    __odom_pub = this->create_publisher<nav_msgs::msg::Odometry>("odom", 10);
    __odom_pos_pub = this->create_publisher<geometry_msgs::msg::PoseWithCovarianceStamped>("pos_odom", 10);
    __hardware_state_pub = this->create_publisher<robot_msgs::msg::Hardware>("hardware_state", 10);

}

/**
 * @brief 初始化订阅者
 *
 * 该函数用于初始化订阅者，订阅名为 "cmd_vel" 的话题，并绑定回调函数 CmdVelCallback。
 *
 */
void RosInterface::InitSubscribers() {
    __cmd_vel_sub = this->create_subscription<geometry_msgs::msg::Twist>(
        "cmd_vel", 10, std::bind(&RosInterface::CmdVelCallback, this, std::placeholders::_1));
    
    __cmd_utils_sub = this->create_subscription<robot_msgs::msg::Hardware>(
        "cmd_utils", 10, std::bind(&RosInterface::CmdUtilsCallback, this, std::placeholders::_1));
    
    
}

/**
 * @brief 初始化定时器
 *
 * 初始化两个定时器，分别用于定期获取串口数据和发送速度命令。
 */
void RosInterface::InitTimers() {
    __get_serial_data_poll_timer = this->create_wall_timer(
        std::chrono::milliseconds(__get_serial_data_poll_interval_ms), 
        std::bind(&RosInterface::GetSerialDataPoll, this));
    __send_velocity_command_timer = this->create_wall_timer(
        std::chrono::milliseconds(__send_velocity_command_poll_interval_ms), 
        std::bind(&RosInterface::SendVelocityCommandPoll, this));
    __odom_pub_poll_timer = this->create_wall_timer(
        std::chrono::milliseconds(__odom_pub_poll_interval_ms), 
        std::bind(&RosInterface::OdomPubPoll, this));
    __hardware_state_pub_poll_timer = this->create_wall_timer(
        std::chrono::milliseconds(__hardware_state_pub_poll_interval_ms), 
        std::bind(&RosInterface::HardwareStatePubPoll, this));
}
/**
 * @brief 初始化硬件接口
 *
 * 该函数用于初始化硬件接口，设置串行端口和波特率，并输出相应的日志信息。
 *
 * @return 无返回值
 */
void RosInterface::InitHardware() {
    __hardware_interface = std::make_shared<HardwareInterface>();
    if (!__hardware_interface->SerialInit(__port, __baudrate)) {
        RCLCPP_ERROR(this->get_logger(), "Failed to initialize serial port: %s", __port.c_str());
    } 
    else {
        RCLCPP_INFO(this->get_logger(), "Serial initialized on %s @ %d baud", __port.c_str(), __baudrate);
    }

}
  
/**
 * @brief 接收速度命令的回调函数
 *
 * 当接收到速度命令时，此函数会被调用。
 *
 * @param msg 包含速度命令的消息
 */
void RosInterface::CmdVelCallback(const geometry_msgs::msg::Twist::SharedPtr msg) {
    // if (!__hardware_interface->IsSerialOK()) {
    //     RCLCPP_WARN(this->get_logger(), "Serial not ready, ignoring cmd_vel");
    //     return;
    // }

    __linear = msg->linear.x;
    __angular = msg->angular.z;
    __last_speed_time = this->get_clock()->now();
}


void RosInterface::CmdUtilsCallback(const robot_msgs::msg::Hardware::SharedPtr msg) {

    __hardware_interface->SendCutterSpdCtlCmd(msg->cutter_spd_ctl);
}


void RosInterface::GetSerialDataPoll() {
    // RCLCPP_INFO(this->get_logger(), "Polling serial data...");
    __hardware_interface->GetSerialData();
    
}


/**
 * @brief 发送速度命令轮询
 *
 * 该函数用于发送速度命令，并处理超时情况。
 *
 * 首先，函数会获取当前时间，并将其存储在成员变量__now_speed_time中。
 * 然后，判断当前时间与上次发送速度命令的时间差是否超过500毫秒（0.5秒）。如果超过，则将线速度和角速度归零。
 * 接下来，根据当前的线速度和角速度计算左右轮的速度。
 * 最后，调用__hardware_interface的SendVelocityCommand函数发送速度命令。
 */
void RosInterface::SendVelocityCommandPoll() {
   
    __now_speed_time = this->get_clock()->now();
    // 超时500ms，速度归零
    if ((__now_speed_time - __last_speed_time) > rclcpp::Duration::from_seconds(0.5)) { 
        __linear = 0.0;
        __angular = 0.0;
    }
 
    double left = __linear - __angular * __track_width / 2.0;
    double right = __linear + __angular * __track_width / 2.0;
    // RCLCPP_INFO(this->get_logger(), "Sending velocity command: left=%f, right=%f", left, right);
    __hardware_interface->SendVelocityCommand(left, right);
}

void RosInterface::OdomPubPoll() { 
    
    try
    {
        EncoderCounts now_encoder_counts = GetEncoderCountsFromHardware();

        // 1. 计算差值
        EncoderCounts delta = now_encoder_counts.deltaFrom(__last_encoder_counts);
        double dt = now_encoder_counts.deltaTimeSec(__last_encoder_counts);
        __last_encoder_counts = now_encoder_counts;

        // 2. 获取当前时间
        auto  now = now_encoder_counts.stamp_ros;

        // 3. 单位转换 mm -> m
        double delta_l_m = delta.left_mm / 1000.0;
        double delta_r_m = delta.right_mm / 1000.0;
        // RCLCPP_INFO(this->get_logger(), "Delta L: %f m, Delta R: %f m", delta_l_m, delta_r_m);

        // 4. 位姿增量计算
        double delta_s = (delta_l_m + delta_r_m) / 2.0;
        double delta_theta = (delta_r_m - delta_l_m) / __track_width;

        // 5. 位姿更新（采用中间角度积分）
        __x += delta_s * std::cos(__theta + delta_theta / 2.0);
        __y += delta_s * std::sin(__theta + delta_theta / 2.0);
        __theta += delta_theta;

        // 6. 速度估计
        double v = (dt > 0) ? (delta_s / dt) : 0.0;
        double w = (dt > 0) ? (delta_theta / dt) : 0.0;

        // 7. 发布里程计消息
        nav_msgs::msg::Odometry odom;
        odom.header.stamp = now;
        odom.header.frame_id = __frame_id;
        odom.child_frame_id = __child_frame_id;

        odom.pose.pose.position.x = __x;
        odom.pose.pose.position.y = __y;
        odom.pose.pose.position.z = 0.0;

        tf2::Quaternion q;
        q.setRPY(0, 0, __theta);
        odom.pose.pose.orientation = tf2::toMsg(q);

        odom.twist.twist.linear.x = v;
        odom.twist.twist.angular.z = w;

        __odom_pub->publish(odom);

        // 8. 发布位姿消息
        geometry_msgs::msg::PoseWithCovarianceStamped pose_msg;
        pose_msg.header = odom.header;
        pose_msg.pose.pose = odom.pose.pose;
        pose_msg.pose.covariance = robot_covariance_config::odom_pose_covariance;

        __odom_pos_pub->publish(pose_msg);

        // 9. 广播 TF
        if(__is_pub_tf)
        {
            geometry_msgs::msg::TransformStamped tf_msg;
            tf_msg.header.stamp = now;
            tf_msg.header.frame_id = __frame_id;
            tf_msg.child_frame_id = __child_frame_id;
            tf_msg.transform.translation.x = __x;
            tf_msg.transform.translation.y = __y;
            tf_msg.transform.translation.z = 0.0;
            tf_msg.transform.rotation = tf2::toMsg(q);

            __tf_broadcaster->sendTransform(tf_msg);
        }
        
    }
    catch(const std::exception& e)
    {
        std::cerr << e.what() << '\n';
    }
}


void RosInterface::HardwareStatePubPoll(){
    __hardware_states = GetHardwareStatesFromHardware();
    
    robot_msgs::msg::Hardware msg;

    // 系统状态
    msg.bat_level = __hardware_states.bat_level;
    msg.bat_voltage = __hardware_states.bat_voltage;
    msg.bat_temperature = __hardware_states.bat_temperature;
    msg.total_current = __hardware_states.total_current;
    msg.core_temperature = __hardware_states.core_temperature;
    msg.sys_temperature = __hardware_states.sys_temperature;

    // 工作状态
    msg.cutter_spd_state = __hardware_states.cutter_spd_state;
    msg.charge_state = __hardware_states.charge_state;
    msg.left_wheel_spd = __hardware_states.left_wheel_spd;
    msg.right_wheel_spd = __hardware_states.right_wheel_spd;
    msg.led_state = __hardware_states.led_state;

    // 传感器状态
    msg.sonar_state = __hardware_states.sonar_state;
  
    // 版本信息
    msg.software_version = __hardware_states.software_version;
    msg.hardware_version = __hardware_states.hardware_version;
    msg.serial_id = __hardware_states.serial_id;

    __hardware_state_pub->publish(msg);
}



EncoderCounts RosInterface::GetEncoderCountsFromHardware() {
    EncoderCounts enc = __hardware_interface->GetMotorEncoderCounts();
    enc.stamp_ros = this->get_clock()->now();
    return enc;
}

HardwareStates RosInterface::GetHardwareStatesFromHardware() {
    HardwareStates hw = __hardware_interface->GetHardwareStates();
    hw.stamp_ros = this->get_clock()->now();
    return hw;
}
